

//import resource.GPSLocation;

import com.ridgesoft.intellibrain.*;
import com.ridgesoft.robotics.*;
import com.ridgesoft.robotics.sensors.*;
import com.ridgesoft.robotics.sensors.CMUcam.CMUcamTrackingData;

public class Navigation implements Runnable {

    private boolean killed = false;
    //private GPSLocation startPoint, targetPoint;
    private DigitalInput direction;
    private String startGPSData = "$GPGGA,123519,45.666271175,N,111.046632,W,1,08,0.9,545.4,M,46.9,M,,*47",
            	   targetGPSData = "$GPGGA,123519,45.666016753,N,111.046686328,W,1,08,0.9,545.4,M,46.9,M,,*47";
    private Sensors data;
    private Movement move;
    private LCD lcd;
    private double heading;
    private boolean isRight;
    private GPS sat;
    private String coords;
    private GPSLocation current, dest;
    private boolean validLoc, navStatus = true;
    private boolean isTracking = false;
    
	int odoCount, rndVal;
	double testVal;
	double cntPerFoot = 2.3;
	
	//whether or not it's directly in front of cone
	boolean atCone = false;
	int midX;
	CMUcamTrackingData trackData;
	Camera cam;
	
    public Navigation() {
    	
    }
    
    public Navigation(Movement m, Sensors s){
    	this.move = m;
    	this.data = s;
    }    
    public Navigation(Movement m, Sensors s, LCD l){
    	this.move = m;
    	this.data = s;
    	this.lcd = l;
		sat = new GPS(lcd);
		cam = new Camera(lcd);
		new Thread(cam).start();
		try {
			Thread.sleep(5000);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

    }
    
    public GPSLocation updateGPS(){
    	validLoc = false;
		while(!validLoc)
		{
			searchForGPS();
		}
		return this.current;
    }
    public void searchForGPS()
    {
    	validLoc = false;
    	coords = sat.updateLocation();
		if(!(coords.equals(""))){
			current = new GPSLocation(coords);
			lcd.printTop("Lat: " + (current.getLatitude()));
			lcd.printBtm("Lng: " + (current.getLongitude()));
			lcd.printTop("dist: " + current.distanceTo(dest));
			validLoc = true;
		}else{
			lcd.printBtm("invalid GPS");
			//Thread.sleep(1000);
		}
		//return validLoc;
    }
    
    boolean straightOn;
    int gotoHeading;
    int currHead;
    public void goToHead(double heading){
        this.avoidObjects();		    		

    	gotoHeading = (int)heading;
    	straightOn = isStraightOn(gotoHeading);
    	
        	if(!straightOn){
        		//turn to heading
        		isRight = findTurnDirection(gotoHeading);
        		if(isRight){
        			//go right
        			move.Right2(200);
        		} 
        		if(!isRight){
        			move.Left2(200);
        		}
        	}
        	move.straight();
        this.navStatus = true;
        this.avoidObjects();		    		        
   }
   
   // CAMERA CONE FINDING STUFF
   ////////////////////////////////////////////////// 
   ////////////////////////////////////////////////// 
   public void findCone(){
	   lcd.printTop("Camera Initialized");
	   cam.initTracking();
	   int speed = 1500;
	   int srvPos;
	   
	   while(!atCone){
		   try{
			srvPos = cam.getServoPos();
			Thread.sleep(250);
			trackData = cam.getTracking();
			if(trackData != null){	
				
				midX = (int)trackData.mx;
				//if forward
				lcd.printBtm("Value: "+midX);
				if(midX > 59 && midX < 120){
					//forward
					turnFromServo(srvPos);
					Thread.sleep(1000);
					continue;
				} else if(midX <= 59 && midX > 0){
					if(srvPos != 0){
						cam.setCamLook(srvPos - 1);
					} else {
						turnFromServo(srvPos);
					}
				} else if(midX >= 120){
					if(srvPos != 4){
						cam.setCamLook(srvPos+ 1);
					} else {
						turnFromServo(srvPos);
					}
				} else if(midX == 0){
					if(srvPos != 0 && srvPos != 4){
						lcd.printTop("lft trn1.1");
						lcd.printBtm("last pos: " + srvPos);
						cam.setCamLook(srvPos - 1);
					}
					else if(srvPos == 4){
						lcd.printTop("lft trn2");
						lcd.printBtm("last pos: " + srvPos);
						cam.setCamLook(srvPos - 1);
					}
					else {
						lcd.printTop("rgt trn1");
						lcd.printBtm("last pos: " + srvPos);
						cam.setCamLook(srvPos + 4);
					}
				}
			}   else {
				lcd.printBtm("Not tracking");
			}
	   		} catch(Exception e){
	   		
	   		}
		}
   }
   
   // turns for a specified time
   public void goLeft(int t){
	   try{
		   move.fwdSlow();
		   move.Left();
		   Thread.sleep(t);
	   } catch(Exception e){
		   e.printStackTrace();
	   } finally {
		   move.straight();
	   }
   }
   
   public void goRight(int t){
	   while(true){
	   try{
		   move.fwdSlow();
		   move.Right();
		   Thread.sleep(t);
	   } catch(Exception e){
		   e.printStackTrace();
	   } finally {
		   move.straight();
		   break;
	   }
	   }
   }
   
   public void foundCone(){
	   lcd.printTop("STOP, FOUND IT");
	   atCone = true;
	   move.stopAllMovement();
   }
   
   
   public void turnFromServo(int pos){
		switch(pos){
		case 0:
			lcd.printTop("go left hard");
			  if((data.getCenter() > 0) && (data.getCenter() < 10)){
				  foundCone();
			  } else {
				  goLeft(1000);
				  cam.setCamLook(2);
			  }
			break;
		case 1:
			lcd.printTop("go left");
			  if((data.getCenter() > 0) && (data.getCenter() < 10)){
				  foundCone();
			  } else {
				goLeft(500);
				 cam.setCamLook(2);
			  }
				break;
		case 2:
			  if((data.getCenter() > 0) && (data.getCenter() < 10)){
				  foundCone();
			  } else {			
				  lcd.printTop("c: "+data.getCenter());
				  move.straight();
				  move.fwdSlow();
			  }
			break;
		case 3:
			lcd.printTop("go right");
			  if((data.getCenter() > 0) && (data.getCenter() < 10)){
				  foundCone();
			  } else {			
				  goRight(500);
				  cam.setCamLook(2);
			  }
			break;
		case 4:
			lcd.printTop("go right hard");
			  if((data.getCenter() > 0) && (data.getCenter() < 10)){
				  foundCone();
			  } else {
				  goRight(1000);
				  cam.setCamLook(2);
			  }
			break;
		}
   }
   
   public void scanCam(){
	   cam.scanLR(1500);
   }
   
   public void lookStraight(){
	   cam.lookFwd();
   }
   
//////////////// ACTUAL MOVEMENT METHODS ///////////////////
   
	public void avoidObjects(){
		move.Forward();
		if((data.getCenter() < 40) && data.getCenter() > 0){
			// try turning right
			try {
				move.Left();					
				Thread.sleep(2000);
			} catch (InterruptedException e) {
				e.printStackTrace();
			} finally {
				move.straight();
			}
		} else {	
			if((data.getLeft() < 40) && (data.getLeft() > 0)){
				//turn right
				try {
					move.Right();					
					Thread.sleep(1000);
				} catch (InterruptedException e) {
					e.printStackTrace();
				} finally {
					move.straight();
				}
			}
			if((data.getRight() < 40) && (data.getRight() > 0)){
				//turn left
				try {
					move.Left();
					Thread.sleep(1000);
				} catch (InterruptedException e) {
					e.printStackTrace();
				} finally {
					move.straight();
				}
			}
		}
	}   
   
   
   
    
   public void goToLoc(double heading, int dist){ //heading to go toward, distance in meters
		if (this.getCurrentHeading() < 0){
			//move.Stop();
			lcd.printTop("WTF STOP!"+this.getCurrentHeading());
			this.navStatus = true;
			//System.out.println("WTF STOP!"+this.getCurrentHeading());
		} else {
			
			lcd.printTop("head: "+(int)data.getHeading());			
			lcd.printBtm("odo: "+data.getOdoCount());			
			goToHead(heading);
			odoCount = data.getOdoCount();
			testVal = (odoCount / cntPerFoot);
			rndVal = (int)(testVal + 0.5);
			//System.out.println(this.getCurrentHeading());

			if(rndVal > dist){
				this.navStatus = false;
				move.Stop();
				return;
			}
		}		   
   }
    
   public boolean checkNavStatus(){
	   return this.navStatus;
   }
   
   public void setNavStatus(boolean b){
	   this.navStatus = b;
   }
    
    //Finds fastest direction to current heading, right is true
    private boolean findTurnDirection(int h){
    	if((((360-h) + (int)data.getHeading()) % 360) > 180)
    		return true;
    	else 
    		return false;
    }

    //if straight onto heading
    private boolean isStraightOn(int h){
    	int current = (int)data.getHeading();
    	int offset = Math.abs(h - current);
    	
    	if((offset > 0) && (offset < 10))
    		return true;
    	else 
    		return false;
    }
    

    
    private void init() 
    {       
    	//startPoint = new GPSLocation(startGPSData);
        //targetPoint = new GPSLocation(targetGPSData);
        //currentPose = new Pose(0, 0, (float) data.getHeading());
    }

    public boolean goingForward() {
        return !direction.isSet();
    }  
    
    public int getCurrentHeading(){
    	return (int)data.getHeading();
    }
    

    public void run() {
        init();
        while (!killed) {
            Thread.yield();
            
            //trackLocation();
        }
    }
}
